#include "WPILib.h"
#include "NetworkTables/NetworkTable.h"
#include "cmath"

class RobotDemo : public IterativeRobot
{
	RobotDrive myRobot;
	Joystick stick1;
	Joystick stick2;
	DoubleSolenoid sol1;
	DoubleSolenoid sol2;
	DoubleSolenoid sol3;
	DoubleSolenoid sol4;
	DoubleSolenoid sol5;
	DoubleSolenoid sol6;
	Compressor * comp;
	Encoder enc;
	Encoder enc2;
	typedef enum AutonomousState {
		Begin = 0, 
		LocateTargets = 10, 
		DeterminePath = 20,
		Drive = 30
		};
	double DistancesFromTargets [3];
	double AnglesToTargets [3];
	double DistancesToWall [3];
	bool PrevButton2;
	bool PrevButton3;
	bool PrevButton4;
	bool PrevButton5;
	bool PrevButton6;
	bool PrevButton7;
	bool StateCompleted;
	int Iter;
	double DistanceToWallFinal;
	double DistanceErrorMeasure;
	double DistanceToGo;
	double DistanceGone;
	double DistancePerEncoderUnit;
	AutonomousState Astate;
	
public:
	
	RobotDemo():
		myRobot(1, 2),
		stick1(1),
		stick2(2),
		sol1(2, 3),
		sol2(4, 5),
		sol3(6, 7),
		sol4(2, 2, 3),
		sol5(2, 4, 5),
		sol6(2, 6, 7),
		enc(2, 3, true, Encoder::k4X),
		enc2(4, 5, true, Encoder::k4X)
	{

		PrevButton2 = false;
		PrevButton3 = false;
		PrevButton4 = false;
		PrevButton5 = false;
		comp = new Compressor(1, 1);
		comp->Start();
		enc.Start();
		enc.Reset();
		enc2.Start();
		enc2.Reset();
		myRobot.SetExpiration(0.1);
		myRobot.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
		myRobot.SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
	}
	void AutonomousInit()
	{
		Astate = Begin;
	}
	void AutonomousPeriodic()
	{
		switch (Astate){
			case Begin:
				Astate = LocateTargets;
				
			case LocateTargets:
				//locating targets code here, get distance and angle
				//DistanceFromTarget1 = Camera.GetDistance(1); // i don't know what it actually is/will be
				//AngleToTarget1 = Camera.GetAngle(1);
				//DistanceFromTarget2 = Camera.GetDistance(2);
				//AngleToTarget2 = Camera.GetAngle(2);
				StateCompleted = true;
				if (StateCompleted)
				{
					Astate = DeterminePath;
					StateCompleted = false;
				}
			case DeterminePath:
				for (Iter = 1; Iter <= 3; Iter++)
				{
					double tdist = DistancesFromTargets[Iter]; 
					double ang = AnglesToTargets[Iter];
					double wdist = tdist*sin(ang);
					DistancesToWall [Iter] = wdist;
				}
				DistanceToWallFinal = (DistancesToWall[1] + DistancesToWall[2] + DistancesToWall[3])/3;
				DistanceErrorMeasure = (fabs(DistancesToWall[1] - DistancesToWall[2]) + fabs(DistancesToWall[2] - DistancesToWall[2]) + fabs(DistancesToWall[1] - DistancesToWall[3]))/3;
				DistanceToGo = 324.88 - DistanceToWallFinal - DistanceErrorMeasure;
				StateCompleted = true;
				if (StateCompleted)
				{
					Astate = Drive;
					StateCompleted = false;
				}
			case Drive:
				DistanceGone = enc.GetDistance();
				if (DistanceGone <= (DistanceToGo - 24))
				{
					myRobot.Drive(.75,0);
				}else if ((DistanceGone >= DistanceToGo - 24)&&(DistanceGone <= DistanceToGo)){
					myRobot.Drive(.25,0);
				}else if (DistanceGone >= DistanceToGo)
				{
					myRobot.Drive(0,0);
				}
				
		}
	}
	void TeleopInit()
	{
		
	}
	void TeleopPeriodic()
	{

		myRobot.TankDrive(&stick1, &stick2);
		SmartDashboard::PutNumber("Encoder 1", enc.GetDistance());
		SmartDashboard::PutNumber("Encoder 2", enc2.GetDistance());
		printf("Encoder 1: %f Encoder 2: %f\n", enc.GetDistance(), enc2.GetDistance()); 
		if((stick1.GetRawButton(2) || stick2.GetRawButton(2))&& !PrevButton2)
		{
			if(sol4.Get() == DoubleSolenoid::kForward)
			{
				sol4.Set(DoubleSolenoid::kReverse);
			}
			else
			{
				sol4.Set(DoubleSolenoid::kForward);
			}
		}
		if((stick1.GetRawButton(3) || stick2.GetRawButton(3))&& !PrevButton3)
		{
			if(sol3.Get() == DoubleSolenoid::kForward)
			{
				sol3.Set(DoubleSolenoid::kReverse);
			}
			else
			{
				sol3.Set(DoubleSolenoid::kForward);
			}
		}

		if((stick1.GetRawButton(4) || stick2.GetRawButton(4))&& !PrevButton4)
		{
			if(sol2.Get() == DoubleSolenoid::kForward)
			{
				sol2.Set(DoubleSolenoid::kReverse);
			}
			else
			{
				sol2.Set(DoubleSolenoid::kForward);
			}
		}
		if((stick1.GetRawButton(5) || stick2.GetRawButton(5))&& !PrevButton5)
		{
			if(sol1.Get() == DoubleSolenoid::kForward)
			{
				sol1.Set(DoubleSolenoid::kReverse);
			}
			else
			{
				sol1.Set(DoubleSolenoid::kForward);
			}
		}
		if((stick1.GetRawButton(6) || stick2.GetRawButton(6))&& !PrevButton6)
		{
			if(sol5.Get() == DoubleSolenoid::kForward)
			{
				sol5.Set(DoubleSolenoid::kReverse);
			}
			else
			{
				sol5.Set(DoubleSolenoid::kForward);
			}
		}
		if((stick1.GetRawButton(7) || stick2.GetRawButton(7))&& !PrevButton7)
		{
			if(sol6.Get() == DoubleSolenoid::kForward)
			{
				sol6.Set(DoubleSolenoid::kReverse);
			}
			else
			{
				sol6.Set(DoubleSolenoid::kForward);
			}
		}
		
		PrevButton3 = (stick1.GetRawButton(3)|| stick2.GetRawButton(3));
		PrevButton2 = (stick1.GetRawButton(2)|| stick2.GetRawButton(2));
		PrevButton4 = (stick1.GetRawButton(4)|| stick2.GetRawButton(4));
		PrevButton5 = (stick1.GetRawButton(5)|| stick2.GetRawButton(5));
		PrevButton6 = (stick1.GetRawButton(6)|| stick2.GetRawButton(6));
		PrevButton7 = (stick1.GetRawButton(7)|| stick2.GetRawButton(7));
	}
		
	~RobotDemo()
	{
		delete comp;
	}
};

START_ROBOT_CLASS(RobotDemo);

